Type TBody Extends TEntity
	Field body:Byte Ptr
	Field node:Byte Ptr
	Field World:TWorld
	Field matrix:Float[16]
	Field Parent:TEntity
	Field Volume:Float
	Field AABB1:Float[3]
	Field AABB2:Float[3]
	Field UpdateForce:Float[3]
	Field Mass:Float
	Field Matrix_Src:Matrix4

	Method Setup()
		Node = newtonCreateBody(World.World , Body) 
		Matrix_Src = Matrix4.Create()
		Local test:Float[] = New Float[16]
		MemCopy(test,Matrix_Src.Pointer(),SizeOf(test))
		NewtonBodySetMatrix(Node ,  test) 
		newtonBodySetUserData(node ,Self)
		Volume = NewtonConvexCollisionCalculateVolume(Body)
		AABB1 = New Float[3]
		AABB2 = New Float[3]
		NewtonBodyGetAABB(Node,AABB1,AABB2)
		World.AddPhysicBody(Self) 
	End Method
	
	Method ResetMatrix()
		Rem
		If mesh.psx <> 1.0 Or mesh.psy <> 1.0 Or mesh.psz <> 1.0 Then
			Local mat:TMatrix = New TMatrix 
			mat.Overwrite(Mesh.Mat) 
			mat.Scale(1.0/mesh.psx,1.0/mesh.psy,1.0/mesh.psz)
			NewtonBodySetMatrix(Node , mat.grid)
			'Print "Resettet"
		Else
			NewtonBodySetMatrix(Node , Mesh.Mat.grid)
		End If
		NewtonBodySetVelocity(Node , [0.0 , 0.0 , 0.0]) 
		NewtonBodySetOmega(Node , [0.0 , 0.0 , 0.0]) 
		End Rem
	End Method
	
	Method Update(mat:Float Ptr)
		Local test:Float[] = New Float[16]
		MemCopy(matrix_src.pointer(),mat,SizeOf(test))
		MemCopy(test,matrix_src.Pointer(),SizeOf(test))
		
		If parent Then
			'SetParentMatrix(test)
			parent.node.setPosition(matrix_src.getTranslation())
			parent.node.SetRotation(matrix_src.getRotationDegrees())
		End If
	End Method
	
	Method SetParentMatrix(m:Float[])
		parent.node.getAbsoluteTransformation().setM(m)
	End Method
	
	
	Method GetParentMatrix:Float[]()
		Local test:Float[] = New Float[16]
		MemCopy(test,parent.GetMatrixPointer(),SizeOf(test))
		Return test
	End Method
	
	Method SetMass(Mass:Float = 0)
		Self.Mass = Mass
		Local Massp:Float[3]
		Local Inertia:Float[3]
		NewtonConvexCollisionCalculateInertialMatrix(body,Inertia,Massp)
		NewtonBodySetCentreOfMass(Node,Massp)
		newtonBodySetMassMatrix(Node , Mass , Mass * inertia[0] , Mass * inertia[1] , Mass * inertia[2])	
	End Method
	
	Method ApplyImpulse(impactX:Float , ImpactY:Float , ImpactZ:Float , ForceX:Float , ForceY:Float , ForceZ:Float) 
		NewtonBodyAddImpulse(node , [ForceX *World.irrMultiplier, ForceY *World.irrMultiplier, ForceZ*World.irrMultiplier] , [impactX, impactY, impactZ]) 
	End Method
	
	Method Save(url:String)
		Local Stream:Int = fopen_(url , "wb") 
     		NewtonCollisionSerialize(Body , Serialize , Byte Ptr Stream) 
	End Method
	
	Method SetParent(entity:TEntity)
		Parent = entity
	End Method
	
	Method UpdateParent()
		If parent Then
			'SetParentMatrix(test)
			parent.node.setPosition(matrix_src.getTranslation())
			parent.node.SetRotation(matrix_src.getRotationDegrees())
		End If
	End Method 
	
	Method Position(x:Float = 0.0, y:Float = 0.0, z:Float = 0.0)
		Self.pos = _VECTOR3DF(x, y, z)
		matrix_src.setTranslation(Self.pos)
		Local test:Float[] = New Float[16]
		MemCopy(test,Matrix_Src.Pointer(),SizeOf(test))
		NewtonBodySetMatrix(Node ,  test) 
		UpdateParent()
	End Method

	
End Type


Type TBodyBox Extends TBody
	
	Function Create:TBodyBox(World:TWorld,SX#=1.0,SY#=1.0,SZ#=1.0)
		Local B:TBodyBox = New TBodyBox
		B.World = World
		B.Body = NewtonCreateBox(World.World , SX , SY , SZ ,Null ) 
		B.Setup() 
		Return B
	End Function
End Type


Type TBodyComplexHull Extends TBody
	
	Function Create:TBodyComplexHull (World:TWorld,src:TEntity,Optimized:Byte)
		 
		Local Mesh:IMesh 
		
		If TAnimMesh(src) <> Null Then
			Local MeshIrr:TAnimMesh = TAnimMesh(src)
			Mesh:IMesh = MeshIrr.msh.GetMesh(0)
		Else If TMesh(src) <> Null Then
			Local MeshIrr:TMesh = TMesh(src)
			Mesh:IMesh = IMesh(MeshIrr.msh)
		Else
			Return Null
		End If
		
		Local B:TBodyComplexHull = New TBodyComplexHull 
		B.Parent = src
		B.World = World
		B.Body = NewtonCreateTreeCollision(World.World) 
		NewtonTreeCollisionBeginBuild(B.Body)
		Local Vector:Float[9]
		Local scale:Vector3df = src.node.GetScale()
		Local cMeshBuffer:Int,j:Int
		Local v1i:Int,v2i:Int,v3i:Int
		Local mb:IMeshBuffer
		
		Local vArray:Float[9]	'vertex array (3*3 floats)
		
		Local tmpCount:Int = 0
		For Local I:Int = 0 To Mesh.getMeshBufferCount()-1
			Local buffer:IMeshBuffer = Mesh.getMeshBuffer(i)
			Local vType:Int = buffer.getVertexType()
			Print vType
			Local Vertices:S3DVertex[] = buffer.getVertices()
			Local Indizes:Short[] = buffer.getIndices()
			Local l1:Int = Indizes.Length
			Local l2:Int = buffer.getIndexCount()
			For Local tris:Int = 0 To l2 -1 Step 3
					
					Local v0:Int=Indizes[tris]
					Local v1:Int=Indizes[tris+1]
					Local v2:Int=Indizes[tris+2]
									
					Local pos:Float[] 
					Local p1:Float[]
					Local p2:Float[]
					Local p3:Int[]
					Local p4:Float[]
					
									
					Vector[0] = Vertices[v0].getPos().getX() * scale.GetX()
					Vector[1] = (Vertices[v0]).getPos().getY() * scale.GetY()
					Vector[2] = (Vertices[v0]).getPos().getZ() * scale.GetZ()
						 
					Vector[3] = (Vertices[v1]).getPos().getX() * scale.GetX()
					Vector[4] = (Vertices[v1]).getPos().getY() * scale.GetY()
					Vector[5] = (Vertices[v1]).getPos().getZ() * scale.GetZ()
						
					Vector[6] = (Vertices[v2]).getPos().getX() * scale.GetX()
					Vector[7] = (Vertices[v2]).getPos().getY() * scale.GetY()
					Vector[8] = (Vertices[v2]).getPos().getZ() * scale.GetZ()
						
					NewtonTreeCollisionAddFace(B.Body,3,Varptr Vector[0],12,tris)		
			Next
			
		Next
		
		If Optimized = True Then 
			NewtonTreeCollisionEndBuild(B.Body , 1) 
		Else
			NewtonTreeCollisionEndBuild(B.Body , 0)
		End If
		B.Setup
		Return B
	End Function
	
	Function Load:TBodyComplexHull(World:TWorld,Url:String , Mesh:TEntity) 
		Local Stream:Int = fopen_(url , "rb") 
		Local B:TBodyComplexHull = New TBodyComplexHull 
		B.Parent = Mesh
		B.World = World
		B.Body = NewtonCreateCollisionFromSerialization(World.World , Deserialize , Byte Ptr stream)
		B.Setup
		Return B
	End Function
End Type

Type TBodyConvexHull Extends TBody
	
	Function Create:TBodyConvexHull(World:TWorld,src:TEntity,tolerance:Float)
	
		Local Mesh:IMesh 
		
		If TAnimMesh(src) <> Null Then
			Local MeshIrr:TAnimMesh = TAnimMesh(src)
			Mesh:IMesh = MeshIrr.msh.GetMesh(0)
		Else If TMesh(src) <> Null Then
			Local MeshIrr:TMesh = TMesh(src)
			Mesh:IMesh = IMesh(MeshIrr.msh)
		Else
			Return Null
		End If

		Local B:TBodyConvexHull = New TBodyConvexHull
		B.PArent = src
		B.World = World
		Local no_verts:Int
		Local vert_coords:Float[]
		Local scale:Vector3df = src.node.GetScale()
		Print "Scale : " + scale.GetX() + " " + scale.GetY() + " " + Scale.GetZ()
		Local P:Int = 0
		
		For Local I:Int = 0 To Mesh.getMeshBufferCount()-1
			Local buffer:IMeshBuffer = Mesh.getMeshBuffer(i)
			Local vType:Int = buffer.getVertexType()
			Local Vertices:S3DVertex[] = buffer.getVertices()
			vert_coords = vert_coords[..vert_coords.length + Vertices.Length*3]
			Local V:Int = 0
			Repeat
				vert_coords[P] = Vertices[V].GetPos().GetX() * scale.GetX()
				vert_coords[P+1] = Vertices[V].GetPos().GetY() * scale.GetY()
				vert_coords[P+2] = Vertices[V].GetPos().GetZ() * scale.GetZ()

				V:+1
				P:+3
			Until V > Vertices.Length -1
			
		Next
	
		B.Body = NewtonCreateConvexHull(World.World , vert_coords.length / 3 , vert_coords , 12 , Tolerance , Null) 
		B.Setup
		Return B
	End Function
End Type

Type TBodySphere Extends TBody
	
	Function Create:TBodySphere(World:TWorld,SX#=1.0,SY#=1.0,SZ#=1.0)
		Local B:TBodySphere = New TBodySphere
		B.World = World
		B.Body = NewtonCreateSphere(World.World,SX,SY,SZ,Null)'NewtonCreateSphere(World.World , SX , SY , SZ , Null) 
		B.Setup() 
		Return B
	End Function
	
End Type


Type TBodyCylinder Extends TBody
	
	Function Create:TBodyCylinder (World:TWorld,SX#=1.0,SY#=1.0)
		Local B:TBodyCylinder = New TBodyCylinder
		B.World = World
		'Local Pivot:TPivot = CreatePivot() 
		'RotateEntity Pivot,0,0,90
		B.Body = NewtonCreateCylinder(World.World,SX,2*SY,Null)'NewtonCreateSphere(World.World , SX , SY , SZ , Null) 
		B.Setup() 
		Return B
	End Function
	
End Type

Type TBodyCone Extends TBody
	
	Function Create:TBodyCone (World:TWorld,SX#=1.0,SY#=1.0)
		Local B:TBodyCone = New TBodyCone 
		B.World = World
		'Local Pivot:TPivot = CreatePivot() 
		'RotateEntity Pivot,0,0,90
		B.Body = NewtonCreateCone(World.World,SX,2*SY,Null)'NewtonCreateSphere(World.World , SX , SY , SZ , Null) 
		B.Setup() 
		Return B
	End Function
	
End Type



Type TBodyCapsule Extends TBody
	
	Function Create:TBodyCapsule (World:TWorld,SX#=1.0,SY#=1.0)
		Local B:TBodyCapsule = New TBodyCapsule 
		B.World = World
		B.Body = NewtonCreateCapsule(World.World,SX,SY,Null)'NewtonCreateSphere(World.World , SX , SY , SZ , Null) 
		B.Setup() 
		Return B
	End Function
	
End Type



Type TBodyChamferCylinder Extends TBody
	
	Function Create:TBodyChamferCylinder (World:TWorld,SX#=1.0,SY#=1.0)
		Local B:TBodyChamferCylinder = New TBodyChamferCylinder 
		B.World = World
		B.Body = NewtonCreateChamferCylinder(World.World,SX,SY,Null)'NewtonCreateSphere(World.World , SX , SY , SZ , Null) 
		B.Setup() 
		Return B
	End Function
	
End Type
